/*
 * mod_filter.h
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */

#ifndef XTFOCLIB_MODULES_MOD_FILTER_H_
#define XTFOCLIB_MODULES_MOD_FILTER_H_

#include "driverlib.h"
#include "bsp_board_config.h"

/** first-order low pass filter************************************************/
typedef struct {
    float LpfK;        /** = Wc * Ts */
    float Output;      /** output */
    float Input;       /** input(previous) */
} stc_1stLpf_t;


/** periodic average filter ***************************************************/
typedef struct {
    float Avg;         /** average output value */
    float Sum;         /** sample value sum */
    float Cnt;         /** current conter */
    float InvTotalCnt ;
} stc_period_avg_t;


// 定义状态向量维度（角度和角速度）
#define STATE_DIM 2
// 定义观测向量维度（仅角度）
#define OBSERV_DIM 1

// 定义EKF结构体
typedef struct {
    float theta;      // 估计的机械角度 (rad)
    float omega;      // 估计的机械角速度 (rad/s)
    float P[2][2];    // 协方差矩阵
    float Q_theta;    // 角度过程噪声
    float Q_omega;    // 角速度过程噪声
    float R_theta;    // 角度观测噪声
} EKF_Estimator;



/**
 *******************************************************************************
 ** \brief: initialize first order low pass filter
 **
 ** \param [in] stc_1stLpf_cfg_t stcCfg: configuration parameters
 ** \param [in] stc_1stLpf_t* pstc: pointer to low-pass filter
 **
 ** \retval     none
 ******************************************************************************/
void Filter_InitFirstLpf(float Fc,float Fs, stc_1stLpf_t *pstc);
//void Filter_InitFirstLpf(stc_1stLpf_cfg_t stcCfg, stc_1stLpf_t *pstc);

/**
 *******************************************************************************
 ** \brief: first order low-pass filter:
 **
 ** \param [in] float i32Q12_Input: input of filter
 ** \param [in] stc_1stLpf_t* pstc: pointer to low-pass filter
 **
 ** \retval     float Output: output of filter
 ******************************************************************************/
float Filter_FirstLpf(float Input, stc_1stLpf_t *pstc);

/**
 *******************************************************************************
 ** \brief: reset states of first order low-pass filter
 **
 ** \param [in] float i32Q12_Output:  set filter ouput
 ** \param [in] stc_1stLpf_t *pstc:     pointer of first order low-pass filter
 **
 ** \retval     none
 ******************************************************************************/
void Filter_RstFirstLpf(float Output, stc_1stLpf_t *pstc);

void Filter_InitPeriodAvg(float avg, stc_period_avg_t *pstc);
/**
 *******************************************************************************
 ** \brief: periodic average filter
 **
 ** \param [in] float Input: input of filter
 ** \param [in] stc_period_avg_t *pstc: pointer to periodic average filter
 **
 ** \retval     float Output: output of filter
 ******************************************************************************/
float Filter_PeriodAvg(float Input, stc_period_avg_t *pstc);

/**
 *******************************************************************************
 ** \brief: reset periodic average filter output
 **
 ** \param [in] float Output:  set onput of filter
 ** \param [in] stc_period_avg_t *pstc: pointer to periodic average filter
 **
 ** \retval     none
 ******************************************************************************/
void Filter_RstPeriodAvg(float Output, stc_period_avg_t *pstc);


void EKF_Init(EKF_Estimator* ekf,
              float init_theta, float init_omega,
              float init_P_theta, float init_P_omega,
              float Q_theta, float Q_omega,
              float R_theta);
void EKF_Predict(EKF_Estimator* ekf, float dt);
void EKF_Update(EKF_Estimator* ekf, float meas_theta);

#endif /* XTFOCLIB_MODULES_MOD_FILTER_H_ */
